#ifndef _SIUT_SIMD_STANDARD_QUATF_HPP_
#define _SIUT_SIMD_STANDARD_QUATF_HPP_

#include <cmath>

#include "siut/simd/Vec3f.hpp"

namespace siut {
namespace simd {
namespace standard {

  class Quatf
  {
  public:
    /*-----------------------------------------------------------------------*/
    /*   Quatf                                                               */
    /*-----------------------------------------------------------------------*/
    
    //constructors
    Quatf();
    Quatf(float, float, float, float);
    Quatf(float tab[4]);
    Quatf(const float *pf);
    Quatf(const Quatf &q);       
    Quatf(const float, const Vec3f &v);
    
    /*-----------------------------------------------------------------------*/
    /* getVector() getScalar() setVector() setScalar()                       */
    /*-----------------------------------------------------------------------*/
    inline float getScalar() const;
    inline float getScalar();

    inline Vec3f getVector() const;
    inline Vec3f getVector();

    inline void setScalar(const float);
    inline void setVector(const Vec3f &v);

    /*-----------------------------------------------------------------------*/
    /*    operators + * += *= [] / /=                                        */
    /*-----------------------------------------------------------------------*/
    //    inline Quatf operator *(const Quatf &a, const Quatf &b);

    inline Quatf operator + (const Quatf &q) const;
    inline Quatf operator * (const Quatf &q) const;
    inline Quatf operator / (const float ) const;
    inline Quatf& operator += (const Quatf &q);
    inline Quatf& operator *= (const Quatf &q);
    inline const float & operator [] (int i) const;
    inline Quatf& operator /= (const float);

    /*-----------------------------------------------------------------------*/
    /*    functions w() x() y() z()                                          */
    /*-----------------------------------------------------------------------*/
    inline const float & w() const;
    inline float & w();
    inline const float & x() const;
    inline float & x();
    inline const float & y() const;
    inline float & y();
    inline const float & z() const;
    inline float & z();

    /*-----------------------------------------------------------------------*/
    /*    functions conjugate() norm() operator ~                            */
    /*-----------------------------------------------------------------------*/
    inline friend Quatf conjugate (const Quatf &q);
    inline Quatf operator~ ();
    inline float norm();
    inline friend float norm(const Quatf &q);

    /*-----------------------------------------------------------------------*/
    /*    functions normalize()                                              */
    /*-----------------------------------------------------------------------*/
    inline Quatf& normalize();
    inline friend Quatf normalize(const Quatf &q);

    
  private:    
    float q_[4];

  };

  Quatf::Quatf()
  {
    //do we want a default constructor with zero rotation?
    q_[0] = 1; q_[1] = q_[2] = q_[3] = 0;
  }
    
  Quatf::Quatf(float w, float x, float y, float z)
  {
    q_[0] = w;
    q_[1] = x;
    q_[2] = y;
    q_[3] = z;
  }

  Quatf::Quatf(float tab[4])
  {
    q_[0] = tab[0];
    q_[1] = tab[1];
    q_[2] = tab[2];
    q_[3] = tab[3];
  }


  Quatf::Quatf(const float *pf)
  {
    q_[0] = pf[0];
    q_[1] = pf[1];
    q_[2] = pf[2];
    q_[3] = pf[3];

  }

  Quatf::Quatf(const Quatf &q)
  {
    q_[0] = q.q_[0];
    q_[1] = q.q_[1];
    q_[2] = q.q_[2];
    q_[3] = q.q_[3];

  }

  Quatf::Quatf(const float s, const Vec3f &v)
  {
    q_[0] = s;
    q_[1] = v.x();
    q_[2] = v.y();
    q_[3] = v.z();
  }

  /*-----------------------------------------------------------------------*/
  /*    operators + * / += *= /=  []                                       */
  /*-----------------------------------------------------------------------*/
  
  inline Quatf Quatf::operator + (const Quatf &q) const
  {
    Quatf res(*this);
    res += q;
    return res;
  }

  
  inline Quatf Quatf::operator * (const Quatf &q) const
  {
    Quatf res = (*this);
    res *= q;
    return  res;
  }

  inline Quatf Quatf::operator / (const float f) const
  {
    Quatf res = (*this);
    res /= f;
    return res;
  }
 
  inline Quatf& Quatf::operator += (const Quatf &q)
  {
    q_[0] += q.q_[0];
    q_[1] += q.q_[1];
    q_[2] += q.q_[2];
    q_[3] += q.q_[3];
    
    return *this;
  }

  inline Quatf& Quatf::operator *= (const Quatf &i)
  {
    float b_[4] = {q_[0], q_[1], q_[2], q_[3]};

    q_[0] = b_[0] * i.q_[0] - (q_[1] * i.q_[1] + b_[2] * i.q_[2] + b_[3]*i.q_[3]);
    q_[1] = b_[0] * i.q_[1] + i.q_[0] * b_[1] + b_[2] * i.q_[3] - i.q_[2] * q_[3];
    q_[2] = b_[0] * i.q_[2] + i.q_[0] * b_[2] + b_[3] * i.q_[1] - i.q_[3] * b_[1];
    q_[3] = b_[0] * i.q_[3] + i.q_[0] * b_[3] + b_[1] * i.q_[2] - i.q_[1] * b_[2];
    
    return *this;
  }

  inline Quatf& Quatf::operator /= (const float f)
  {
    q_[0] /= f;
    q_[1] /= f;
    q_[2] /= f;
    q_[3] /= f;
    
    return *this;
  }

  inline const float & Quatf::operator [] (int i) const
  {
    return q_[i];
  }

  /*-----------------------------------------------------------------------*/
  /*    functions w() x() y() z() vec_[]()                                    */
  /*-----------------------------------------------------------------------*/

  inline const float & Quatf::w() const
  {
    return q_[0];
  }

  inline const float & Quatf::x() const
  {
    return q_[1];
  }

  inline const float & Quatf::y() const
  {
    return q_[2];
  }

  inline const float & Quatf::z() const
  {
    return q_[3];
  }

  inline float & Quatf::w()
  {
    return q_[0];
  }

  inline float & Quatf::x()
  {
    return q_[1];
  }

  inline float & Quatf::y()
  {
    return q_[2];
  }

  inline float & Quatf::z()
  {
    return q_[3];
  }

 
  /*-----------------------------------------------------------------------*/
  /* getVector() getScalar() setVector() setScalar()                       */
  /*-----------------------------------------------------------------------*/
  inline float Quatf::getScalar() const
  {
    return q_[0];
  }
  inline float Quatf::getScalar()
  {
    return q_[0];
  }

  inline Vec3f Quatf::getVector() const
  {
    return Vec3f(q_[1], q_[2], q_[3]);
  }
  inline Vec3f Quatf::getVector()
  {
    return Vec3f(q_[1], q_[2], q_[3]);
  }

  inline void Quatf::setScalar(const float f)
  {
    q_[0] = f;
  }

  inline void Quatf::setVector(const Vec3f &v)
  {
    q_[1] = v[0];
    q_[2] = v[1];
    q_[3] = v[2];
  }

  /*-----------------------------------------------------------------------*/
  /*    functions conjugate() norm() operator ~                            */
  /*-----------------------------------------------------------------------*/
  inline Quatf conjugate (const Quatf &q)
  {
    return Quatf(q.q_[0], -q.q_[1], -q.q_[2], -q.q_[3]);
  }

  inline Quatf Quatf::operator~ ()
  {
    return Quatf(q_[0], -q_[1], -q_[2], -q_[3]);
  }
  
  inline float Quatf::norm() 
  {
    return std::sqrt(pow(q_[0],2)+pow(q_[1],2)+pow(q_[2],2)+pow(q_[3],2));
  }

  inline float norm(const Quatf &q)
  {
    return std::sqrt(pow(q.q_[0],2)+pow(q.q_[1],2)+pow(q.q_[2],2)+pow(q.q_[3],2));
  }

  /*-----------------------------------------------------------------------*/
  /*    functions normalize()                                              */
  /*-----------------------------------------------------------------------*/
  inline Quatf& Quatf::normalize()
  {
    float f = (*this).norm();
    (*this) /= f;
    return (*this);
  }

  inline Quatf normalize(const Quatf &q)
  {
    float f = norm(q);
    Quatf ret(q);
    ret /= f;
    return ret;
  }
  
}
}
}

#endif // _SIUT_SIMD_STANDARD_QUATF_HPP_
